#include "motor.h"
#include "main.h"
#include "stdio.h"

extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;

int16_t motor_speed[2];

void motor_init()
{
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0);

	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);

    HAL_GPIO_WritePin(GPIOB, IN1_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, IN2_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, IN3_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, IN4_Pin,GPIO_PIN_RESET);
    
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
    
}

void timer1_inter(void)
{
//    uint16_t direction0 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim4); 
//    uint16_t direction1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim3); 
//    
//    uint16_t captureNumber0 =__HAL_TIM_GET_COUNTER(&htim4);    
//    uint16_t captureNumber1 =__HAL_TIM_GET_COUNTER(&htim3); 
//    

//    __HAL_TIM_SET_COUNTER(&htim4,0);
//    __HAL_TIM_SET_COUNTER(&htim3,0);
//    
//    if(direction0 == 1 && captureNumber0 != 0)
//    {
//        captureNumber0 = 65535 - captureNumber0;
//    }
//    
//    if(direction1 == 1 && captureNumber1 != 0)
//    {
//        captureNumber1 = 65535 - captureNumber1;
//    }  
//    
//    if(direction0 == 1)
//        motor_speed[0] = captureNumber0;
//    else
//        motor_speed[0] = -captureNumber0;
//    
//    if(direction1 == 1)
//        motor_speed[1] = captureNumber1;
//    else
//        motor_speed[1] = -captureNumber1;
    
//    if(captureNumber0 != 0 || captureNumber1 != 0)
//        printf("%d\t%d\r\n", motor_speed[0],motor_speed[1]);
    

}

void motor_ctrl(int8_t speed0,int8_t speed1)
{
    if(speed0 > MAX_SPEED)
        speed0 = MAX_SPEED;
    else if(speed0 < -MAX_SPEED)
        speed0 = -MAX_SPEED;

    if(speed1 > MAX_SPEED)
        speed1 = MAX_SPEED;
    else if(speed1 < -MAX_SPEED)
        speed1 = -MAX_SPEED;
    
    if(speed0 < MIN_SPEED && speed0 > -MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB, IN2_Pin,GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);  
    }else if(speed0 >= MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB, IN2_Pin,GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed0);  
    }else if(speed0 <= -MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB, IN2_Pin,GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, -speed0);  
    }
    
    if(speed1 < MIN_SPEED && speed1 > -MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN4_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB, IN3_Pin,GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0);  
    }else if(speed1 >= MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN4_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB, IN3_Pin,GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed1);  
    }else if(speed1 <= -MIN_SPEED)
    {
        HAL_GPIO_WritePin(GPIOB, IN4_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB, IN3_Pin,GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, -speed1);  
    }    
}

void motor_test()
{
    
//    HAL_GPIO_WritePin(GPIOB, IN1_Pin,GPIO_PIN_SET);
//    HAL_GPIO_WritePin(GPIOB, IN2_Pin,GPIO_PIN_RESET);
//    
//    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 30);    
//    
//    HAL_GPIO_WritePin(GPIOB, IN3_Pin,GPIO_PIN_SET);
//    HAL_GPIO_WritePin(GPIOB, IN4_Pin,GPIO_PIN_RESET);
//    
//    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 30);   
    
}